{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "intro_md"
      },
      "source": [
        "# InitializePose3"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "desc_md"
      },
      "source": [
        "The `InitializePose3` structure provides static methods for computing an initial estimate for 3D poses (`Pose3`) in a factor graph, particularly useful for Structure from Motion (SfM) or SLAM problems.\n",
        "The core idea is to first estimate the orientations (`Rot3`) independently and then use these estimates to initialize a linear system for the translations.\n",
        "\n",
        "Key static methods:\n",
        "*   `buildPose3graph(graph)`: Extracts the subgraph containing only `Pose3` `BetweenFactor` and `PriorFactor` constraints from a larger `NonlinearFactorGraph`.\n",
        "*   `computeOrientationsChordal(pose3Graph)`: Estimates rotations using chordal relaxation on the rotation constraints.\n",
        "*   `computeOrientationsGradient(pose3Graph, initialGuess)`: Estimates rotations using gradient descent on the manifold.\n",
        "*   `initializeOrientations(graph)`: Convenience function combining `buildPose3graph` and `computeOrientationsChordal`.\n",
        "*   `computePoses(initialRot, poseGraph)`: Computes translations given estimated rotations by solving a linear system (performing one Gauss-Newton iteration on poses).\n",
        "*   `initialize(graph)`: Performs the full initialization pipeline (extract graph, estimate rotations via chordal, compute translations).\n",
        "*   `initialize(graph, givenGuess, useGradient)`: Full pipeline allowing specification of an initial guess and choosing between chordal or gradient descent for rotations."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "colab_badge_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/InitializePose3.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "%pip install --quiet gtsam-develop"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "imports_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "from gtsam import NonlinearFactorGraph, Values, Pose3, Rot3, Point3, PriorFactorPose3, BetweenFactorPose3\n",
        "from gtsam import InitializePose3\n",
        "from gtsam import symbol_shorthand\n",
        "\n",
        "X = symbol_shorthand.X"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "example_header_md"
      },
      "source": [
        "## Example Initialization Pipeline"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "example_desc_md"
      },
      "source": [
        "We'll create a simple 3D pose graph and use the `InitializePose3.initialize` method to get an initial estimate."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "id": "example_pipeline_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Original Factor Graph:\n",
            "size: 4\n",
            "\n",
            "Factor 0: PriorFactor on x0\n",
            "  prior mean:  R: [\n",
            "\t0.995004, -0.0998334, 0;\n",
            "\t0.0998334, 0.995004, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t: 0.1   0   0\n",
            "  noise model: diagonal sigmas [0.01; 0.01; 0.01; 0.05; 0.05; 0.05];\n",
            "\n",
            "Factor 1: BetweenFactor(x0,x1)\n",
            "  measured:  R: [\n",
            "\t0.877582562, -0.479425539, 0;\n",
            "\t0.479425539, 0.877582562, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t:   1 0.1   0\n",
            "  noise model: diagonal sigmas [0.05; 0.05; 0.05; 0.2; 0.2; 0.2];\n",
            "\n",
            "Factor 2: BetweenFactor(x1,x2)\n",
            "  measured:  R: [\n",
            "\t0.921060994, -0.389418342, 0;\n",
            "\t0.389418342, 0.921060994, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t:  0.9 -0.1    0\n",
            "  noise model: diagonal sigmas [0.05; 0.05; 0.05; 0.2; 0.2; 0.2];\n",
            "\n",
            "Factor 3: BetweenFactor(x2,x0)\n",
            "  measured:  R: [\n",
            "\t0.621609968, 0.78332691, 0;\n",
            "\t-0.78332691, 0.621609968, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t: -1.8 0.05    0\n",
            "  noise model: diagonal sigmas [0.1; 0.1; 0.1; 0.4; 0.4; 0.4];\n"
          ]
        },
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "\n",
            "Initial Estimate (using InitializePose3.initialize):\n",
            "\n",
            "Values with 3 values:\n",
            "Value x0: (class gtsam::Pose3)\n",
            "R: [\n",
            "\t0.995004165, -0.0998334166, 0;\n",
            "\t0.0998334166, 0.995004165, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t:             0.1 -7.63278329e-15               0\n",
            "\n",
            "Value x1: (class gtsam::Pose3)\n",
            "R: [\n",
            "\t0.825335615, -0.564642473, 0;\n",
            "\t0.564642473, 0.825335615, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t: 0.956742586 0.343109526           0\n",
            "\n",
            "Value x2: (class gtsam::Pose3)\n",
            "R: [\n",
            "\t0.540302306, -0.841470985, 0;\n",
            "\t0.841470985, 0.540302306, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t:  1.62773065 0.912529884           0\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# 1. Create a NonlinearFactorGraph with Pose3 factors\n",
        "graph = NonlinearFactorGraph()\n",
        "\n",
        "# Add a prior on the first pose\n",
        "prior_mean = Pose3(Rot3.Yaw(0.1), Point3(0.1, 0, 0))\n",
        "prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.01]*3 + [0.05]*3))\n",
        "graph.add(PriorFactorPose3(X(0), prior_mean, prior_noise))\n",
        "\n",
        "# Add odometry factors\n",
        "odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.05]*3 + [0.2]*3))\n",
        "odometry1 = Pose3(Rot3.Yaw(0.5), Point3(1.0, 0.1, 0))\n",
        "odometry2 = Pose3(Rot3.Yaw(0.4), Point3(0.9, -0.1, 0))\n",
        "graph.add(BetweenFactorPose3(X(0), X(1), odometry1, odometry_noise))\n",
        "graph.add(BetweenFactorPose3(X(1), X(2), odometry2, odometry_noise))\n",
        "\n",
        "# Add a loop closure factor (less certain)\n",
        "loop_mean = Pose3(Rot3.Yaw(-0.9), Point3(-1.8, 0.05, 0))\n",
        "loop_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1]*3 + [0.4]*3))\n",
        "graph.add(BetweenFactorPose3(X(2), X(0), loop_mean, loop_noise))\n",
        "\n",
        "graph.print(\"Original Factor Graph:\\n\")\n",
        "\n",
        "# 2. Perform initialization using the default chordal relaxation method\n",
        "initial_estimate = InitializePose3.initialize(graph)\n",
        "\n",
        "print(\"\\nInitial Estimate (using InitializePose3.initialize):\\n\")\n",
        "initial_estimate.print()\n",
        "\n",
        "# 3. (Optional) Perform initialization step-by-step\n",
        "# pose3graph = InitializePose3.buildPose3graph(graph)\n",
        "# initial_orientations = InitializePose3.initializeOrientations(graph)\n",
        "# initial_estimate_manual = InitializePose3.computePoses(initial_orientations, pose3graph)\n",
        "# print(\"\\nInitial Estimate (Manual Steps):\\n\")\n",
        "# initial_estimate_manual.print()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "notes_header_md"
      },
      "source": [
        "## Notes"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "notes_desc_md"
      },
      "source": [
        "- The quality of the initial estimate depends heavily on the quality and connectivity of the pose graph factors.\n",
        "- Chordal relaxation (`computeOrientationsChordal`) is generally faster and often sufficient.\n",
        "- Gradient descent (`computeOrientationsGradient`) might provide slightly more accurate orientations but is slower and requires a good initial guess.\n",
        "- The final `computePoses` step performs only a *single* Gauss-Newton iteration, assuming the rotations are fixed. The resulting estimate is meant as an initialization for a full nonlinear optimization (e.g., using `GaussNewtonOptimizer` or `LevenbergMarquardtOptimizer`)."
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "gtsam",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.13.1"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}
